﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Research.Kinect.Nui;
using Blake.NUI.WPF.Utility;
using System.Diagnostics;
using Rect = System.Windows.Rect;

namespace Kinductor.Gestures
{

    enum ConductingStyle
    {
        Normal,
        Legato,
        Staccato
    }

    enum HandConductingPhase
    {
        NotConducting,
        Steady,
        MarkingTime,
        Volume,
        Unknown
    }

    class HandConductingStatus
    {
        public HandConductingPhase Phase { get; set; }
        public double BeatsPerMinute { get; set; }
        public DateTime LastIctusTime { get; set; }
        public TimeSpan IctusDelta { get; set; }
        public Vector LastIctusPosition { get; set; }

        public Rect BeatPatternBounds { get; private set; }
        public double Energy { get; private set; }

        public Vector LastWristPosition;
        public Vector LastWristVelocity;
        public Vector LastWristAcceleration;

        public Vector LastShoulderPosition;
        public double VolumeDelta;

        private Queue<Vector> HandVelocitySmoothing;
        private Vector HandMovementHistorySum;
        private Vector RecentAverageHandVelocity;
        private Vector OlderAverageHandVelocity;
        private Vector LongAverageHandVelocity;

        private Queue<double> BeatsPerMinuteSmoothing;

        public bool IsPulsing { get; set; }

        private TimeSpan pulsingTimeout = TimeSpan.FromSeconds(1.5);
        private DateTime lastUpdate = DateTime.MinValue;

        private double steadyMovementThreshold = 0.1;

        private double maxBeatPatternY = 0;

        private bool IsLeftHand = false;

        private double lastVolumeRelativeY = 0;

        public HandConductingStatus()
        {
            Phase = HandConductingPhase.NotConducting;
            LastIctusTime = DateTime.MinValue;
            LastIctusPosition = new Vector();

            LastWristPosition = new Vector();
            LastWristVelocity = new Vector();
            LastWristAcceleration = new Vector();

            HandVelocitySmoothing = new Queue<Vector>();
            RecentAverageHandVelocity = new Vector();
            OlderAverageHandVelocity = new Vector();

            BeatsPerMinuteSmoothing = new Queue<double>();
        }

        public void UpdateStatus(Joint hand, Joint wrist, Joint hip, Joint shoulder)
        {
            TimeSpan dt = DateTime.Now - lastUpdate;
            IntegratePositions(hand, wrist, dt);
            IsLeftHand = hand.ID == JointID.HandLeft;

            LastShoulderPosition = shoulder.Position;

            lastUpdate = DateTime.Now;

            if (RecognizeNotConducting(hip, shoulder))
            {
                return;
            }

            if (IsLeftHand)
            {
                if (RecognizeSteady())
                {
                    return;
                }

                if (RecognizeVolume())
                {
                    return;
                }
            }
            else
            {
                if (RecognizeMarkingTime())
                {
                    return;
                }
            }

            Phase = HandConductingPhase.Unknown;
        }

        private bool RecognizeNotConducting(Joint hip, Joint shoulder)
        {
            //LastWristPosition.Z > shoulder.Position.Z - .1 &&
                
            if (LastWristPosition.Y < hip.Position.Y)
            {
                Phase = HandConductingPhase.NotConducting;
                return true;
            }
            return false;
        }

        private bool RecognizeSteady()
        {
            var movement = LastWristVelocity.Magnitude();

            if (IsLeftHand && 
                LastWristPosition.X < LastShoulderPosition.X - 0.2)
            {
                if (movement < steadyMovementThreshold)
                {
                    Phase = HandConductingPhase.Steady;
                    lastVolumeRelativeY = LastWristPosition.Y;
                    return true;
                }
            }

            return false;
        }

        private bool RecognizeVolume()
        {
            var movement = HandMovementHistorySum.Magnitude();

            var velocity = RecentAverageHandVelocity.Magnitude();
            var relativeY = LastWristPosition.Y;
            if (Math.Abs(LastWristVelocity.X) < steadyMovementThreshold &&
                Math.Abs(LastWristVelocity.Z) < steadyMovementThreshold &&
                (Phase == HandConductingPhase.Steady || Phase == HandConductingPhase.Volume))
            {
                //if (Phase != HandConductingPhase.Volume)
                //{
                //    lastVolumeRelativeY = LastWristPosition.Y;
                //}
                Phase = HandConductingPhase.Volume;
                //double deltaY = (LastWristPosition.Y - lastVolumeRelativeY);  
                if (LastWristPosition.X < LastShoulderPosition.X - 0.2)
                {
                    VolumeDelta = LastWristPosition.Y - LastShoulderPosition.Y;
                }
                lastVolumeRelativeY = LastWristPosition.Y;

                return true;
            }
            return false;
        }

        private bool RecognizeMarkingTime()
        {
            TimeSpan delta = DateTime.Now - LastIctusTime;
            var bpm = 1 / delta.TotalMinutes;
            var movement = HandMovementHistorySum.Magnitude();

            if (movement > 1.0 &&
                RecentAverageHandVelocity.Y > 0 &&
                OlderAverageHandVelocity.Y < 0)
            {
                if (!IsPulsing)
                {
                    IsPulsing = true;
                    LastIctusTime = DateTime.Now;
                    LastIctusPosition = LastWristPosition;
                    IctusDelta = delta;

                    maxBeatPatternY = LastWristPosition.Y;

                    if (bpm > 40 && bpm < 200)
                    {
                        if (Phase != HandConductingPhase.MarkingTime)
                        {
                            Energy = 0;
                        }
                        Phase = HandConductingPhase.MarkingTime;

                        if (BeatsPerMinuteSmoothing.Count > 3)
                        {
                            BeatsPerMinuteSmoothing.Dequeue();
                        }

                        BeatsPerMinuteSmoothing.Enqueue(bpm);

                        var avgBPM = BeatsPerMinuteSmoothing.Average();

                        BeatsPerMinute = GetClosestBpmBin(avgBPM);
                    }

                }
            }
            else
            {
                IsPulsing = false;
            }

            if (Phase == HandConductingPhase.MarkingTime)
            {
                if (LastWristPosition.Y >= maxBeatPatternY)
                {
                    maxBeatPatternY = LastWristPosition.Y;
                }
                else
                {
                    double deltaY = maxBeatPatternY - LastIctusPosition.Y;
                    Energy = MathUtility.Clamp(MathUtility.MapValue(deltaY, .1, .3, 0, 1), 0, 1);
                }
            }

            if (Phase == HandConductingPhase.MarkingTime &&
                delta > pulsingTimeout)
            {
                BeatsPerMinute = 0;
                BeatsPerMinuteSmoothing.Clear();
                Phase = HandConductingPhase.Unknown;
                Energy = 0;
            }

            return Phase == HandConductingPhase.MarkingTime;
        }

        private double GetClosestBpmBin(double avgBPM)
        {
            int interval = 10;
            for (int i = 65; i <= 205; i += interval)
            {
                if (avgBPM <= i)
                    return i - interval / 2;
            }

            return 200;
        }

        private void IntegratePositions(Joint hand, Joint wrist, TimeSpan delta)
        {
            double dt = delta.TotalSeconds;
            Vector relativeHandPosition = hand.Position;

            Vector newHandVelocity = relativeHandPosition.Subtract(LastWristPosition).Divide(dt);
            LastWristAcceleration = newHandVelocity.Subtract(LastWristVelocity).Divide(dt);
            LastWristPosition = relativeHandPosition;
            LastWristVelocity = newHandVelocity;

            if (HandVelocitySmoothing.Count > 6)
                HandVelocitySmoothing.Dequeue();
            HandVelocitySmoothing.Enqueue(LastWristVelocity);

            RecentAverageHandVelocity = new Vector();
            OlderAverageHandVelocity = new Vector();
            LongAverageHandVelocity = new Vector();
            HandMovementHistorySum = new Vector();

            if (HandVelocitySmoothing.Count >= 4)
            {
                var velocities = HandVelocitySmoothing.ToList();

                foreach (var v in velocities.Skip(3).Take(3))
                {
                    RecentAverageHandVelocity = RecentAverageHandVelocity.Add(v);
                }

                RecentAverageHandVelocity = RecentAverageHandVelocity.Divide(3);

                foreach (var v in velocities.Skip(2).Take(3))
                {
                    OlderAverageHandVelocity = OlderAverageHandVelocity.Add(v);
                }

                OlderAverageHandVelocity = OlderAverageHandVelocity.Divide(3);

                foreach (var v in velocities)
                {
                    HandMovementHistorySum = HandMovementHistorySum.Add(v);
                }

                LongAverageHandVelocity = HandMovementHistorySum.Divide(velocities.Count);
            }
        }

    }
}
